#include "hal_flash.h"


static uint32_t get_page(uint32_t Addr)
{
    uint32_t page = 0;

    if (Addr < (FLASH_BASE + FLASH_BANK_SIZE))
    {
        /* Bank 1 */
        page = (Addr - FLASH_BASE) / FLASH_PAGE_SIZE;
    }
    else
    {
        /* Bank 2 */
        page = (Addr - (FLASH_BASE + FLASH_BANK_SIZE)) / FLASH_PAGE_SIZE;
    }

    return page;
}

static uint32_t get_bank(uint32_t Addr)
{
    if ((FLASH_BASE <= Addr) && (Addr < (FLASH_BASE + FLASH_BANK_SIZE)))
        return FLASH_BANK_1;
    else
        return FLASH_BANK_2;
}

int32_t hal_flash_erase(uint32_t erase_addr, uint32_t size)
{
    uint32_t FirstPage = 0;
    uint32_t NbOfPages = 0;
    uint32_t BankNumber = 0;
    uint32_t BankStart = 0;
    uint32_t BankEnd = 0;
    uint32_t PageError = 0;

    FLASH_EraseInitTypeDef EraseInitStruct;
    
    BankStart = get_bank(erase_addr);
    BankEnd = get_bank(erase_addr + size);
    
    HAL_FLASH_Unlock();

    if (BankStart != BankEnd)
    {
        /* Get the 1st page to erase */
        FirstPage = get_page(erase_addr);

        /* Get the number of pages to erase from 1st page */
        NbOfPages = (size / FLASH_PAGE_SIZE) + ((size % FLASH_PAGE_SIZE) ? 1 : 0);

        /* Get the bank */
        BankNumber = get_bank(erase_addr);

        /* Fill EraseInit structure*/
        EraseInitStruct.TypeErase   = FLASH_TYPEERASE_PAGES;
        EraseInitStruct.Banks       = BankNumber;
        EraseInitStruct.Page        = FirstPage;
        EraseInitStruct.NbPages     = NbOfPages;

        if (HAL_FLASHEx_Erase(&EraseInitStruct, &PageError) != HAL_OK)
        {
            return -1;
        }
        
        FirstPage = get_page(FLASH_BASE + FLASH_BANK_SIZE);

        NbOfPages = (size / FLASH_PAGE_SIZE) + ((size % FLASH_PAGE_SIZE) ? 1 : 0);
        BankNumber = get_bank(erase_addr);
        EraseInitStruct.TypeErase   = FLASH_TYPEERASE_PAGES;
        EraseInitStruct.Banks       = BankNumber;
        EraseInitStruct.Page        = FirstPage;
        EraseInitStruct.NbPages     = NbOfPages;
        if (HAL_FLASHEx_Erase(&EraseInitStruct, &PageError) != HAL_OK)
        {
            return -1;
        }
    }
    else
    {
        FirstPage = get_page(erase_addr);
        NbOfPages = (size / FLASH_PAGE_SIZE) + ((size % FLASH_PAGE_SIZE) ? 1 : 0);
        BankNumber = get_bank(erase_addr);
        EraseInitStruct.TypeErase   = FLASH_TYPEERASE_PAGES;
        EraseInitStruct.Banks       = BankNumber;
        EraseInitStruct.Page        = FirstPage;
        EraseInitStruct.NbPages     = NbOfPages;
        if (HAL_FLASHEx_Erase(&EraseInitStruct, &PageError) != HAL_OK)
        {
            return -1;
        }
    }
    HAL_FLASH_Lock();

    return 0;
}

int32_t hal_flash_read(uint8_t *p_data, uint32_t read_addr, uint32_t size)
{
    uint32_t addr = read_addr;
    uint32_t addr_end = read_addr + size;

    while (addr < addr_end)
    {
        *(uint32_t *)p_data = *(__IO uint32_t *)addr;
        addr = addr + 4;
        p_data = p_data + 4;
    }
}

int32_t hal_flash_write(uint8_t *p_data, uint32_t write_addr, uint32_t size)
{
    uint32_t addr = write_addr;
    uint32_t addr_end = write_addr + size;
    
    HAL_FLASH_Unlock();
    
    while (addr < addr_end)
    {
        if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, addr, *(uint64_t *)p_data) == HAL_OK)
        {
            addr = addr + 8;
            p_data = p_data + 8;
        }
       else
        {
            printf("FLASH Program Error!!!\n");
            return -1;
        }
    }
    
    HAL_FLASH_Lock();
    
    return 0;
}
